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ABSTRACT 



The problem of path keeping of marine vehicles along 
curved paths is considered. In particular, we are concerned 
with circular arcs that join two consecutive straight line 
segments. This provides smooth path changes between one 
segment and the next utilizing a purely geometric reference 
path construction. Pure pursuit guidance is coupled with 
orientation angle control law to ensure stability and 
accuracy. Sensitivity analysis with respect to inaccuracies in 
the knowledge of the vehicle hydrodynamic characteristics is 
performed. The results demonstrate the validity of this 
approach and offer a way to achieve accurate path keeping in 
confined spaces. 
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I. 



INTRODUCTION 



One of the most important functions of a marine vehicle is 
accurate path keeping through prescribed routes. Path accuracy 
is particularly important when the vehicle has to operate in 
confined waters. In the case of an unmanned vehicle, the 
commanded path is usually approximated by straight line 
segments between consecutive way points. This assumption works 
well if the desired route is a smooth path and can be 
approximated by a series of straight line segments. This has 
been studied extensively in the past. Lienard [1] designed a 
steering sliding mode control autopilot based on the line of 
sight, while Chism [2] studied a cross track error based 
control law. Hawkinson [3] extended the results to the 
multiple input case when bow and stern planes are 
independently controlled. Instead of using the cross track 
error directly in the control law, similar path keeping 
characteristics can be achieved by using a line of sight 
pursuit guidance scheme. This is accomplished by moving the 
vehicle in the direction of a moving way point which is 
located on the commanded straight line path [4]. Stability of 
this scheme depends on the control law gains and the look 
ahead distance between the vehicle and the moving way point 
[5]. The technique can be applied to combined steering and 
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diving response [6]. In this work, we extend the previous 
studies of pursuit guidance in the case of curved reference 
paths with piecewise constant curvature. This means that we 
adopt a purely geometrical construction of the reference path 
which is composed of a series of straight line segments and 
circular arcs. In Chapter II we present an overview of the 
equations of motion in the horizontal plane, the control and 
the guidance laws. We use the full set of maneuvering 
equations for simulation and a reduced model, Nomoto's first 
order model, for steering control design. This is because the 
vehicle parameters in the reduced order model can be estimated 
relatively accurately from first principles and experimental 
data [7]. In Chapter III the focus is on stability. We present 
two stability conditions, one based on the reduced model, and 
one based on the complete vehicle dynamics model. The 
sensitivity of the stability curves with respect to the 
vehicle hydrodynamic coefficients and control parameters is 
also studied in this chapter. In Chapter IV we present the 
development and simulation results of the guidance scheme for 
circular reference paths. All computations in this work are 
performed for the NPS AUV II vehicle for which a complete set 
of hydrodynamic coefficients and geometric properties is 
available [8] . Finally, we summarize the conclusions of this 
work and some recommendations for further research. 
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II. MATHEMATICAL FORMULATION 



In this section the vehicle equations of motion for the 
horizontal plane (x,y), the control law and the guidance 
scheme are presented. 

A. EQUATIONS OF MOTION 

For the horizontal plane the mathematical model consists 
of the following simplified expressions for the nonlinear sway 
and yaw differential equations shown below : 



Equations (2.1), (2.2) were derived from the general six 

degrees of freedom equations for a vehicle by setting all 
terms related to the vertical plane motion to be zero, 
assuming port /starboard symmetry. The equations for the sway 
force Y and yaw moment N are : 



m( v+ur+X(j/) =Y 



( 2 . 1 ) 



I +ttiXq (v+ur) -N 



( 2 . 2 ) 



( Y^v+Y^ur) +Y^uv+Y^u^b 



N=Nj,Y+ iN^v+Nj.ur) +N^uv+N^u^6 
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Finally the three kinematic equations of the inertial position 
rates and yaw rate are : 



ilf=r 


(2.3) 


x= uc 0 s i|; - vs i n i|; 


(2.4) 


y = us i ni|r + vco s ijr 


(2.5) 



B . CONTROL LAW 

The coupled equations (2.1) and (2.2) after some algebra 
appear as shown : 



v=a^^ u v+a^2 ^ 


(2.6) 


f =aj 1 u v+a2 2 ut +2)2 LI ^ ^ 


(2.7) 



where the coefficients a^, ajj# ^21/ ^^^^1 b2 are : 

D={I^-Ni) {m-Y^) -{mxQ-Y^) (mx^-N^) 

aii=^ [ Y^-(mx^-Y,)N^] 

ai2=-^ [ {-m^Y^) -{mXg-Y^) {-mXg+N^) ] 

^21=^ {m-Y^iN^-imXg-N^) yj 
^22=-^ [ (/n-y^^) {-mXg+N^) -{mXg-N^) {-m+Yj.) ] 
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^1 = 3 t <rnx^-Y,) (mx^-Y,) N^J 

i?2=-^ [ im-Y^) N^- (mXg-N^) Y^- im-Y^) (mx^-N^) Y^J 



Nondimensionilizing the above equations in terms of the water 
density p the vehicle length L and the nominal forward speed 
U of the vehicle, results in the following : 



v=a^^v+aj^2^ 


(2.8) 


f=a2iV+a22r+b2S 


(2.9) 



From equations (2.8) and (2.9), the relationship of the 
vehicle turning rate r and the rudder angle 6 appears through 
a second order transfer function : 



_r ^11^2 

6 s^-(a22*a^^)s*a^^a22-a^2^2i 



( 2 . 10 ) 



Performing a Taylor series expansion in the inverse of 
equation (2.10) and keeping the first two terms we arrive at 
a simplified first order transfer function : 



r_ 



C 1 +C 2 S 



( 2 . 11 ) 



where : 
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Cj= ( 32 ^ 1^22 ^21^12^ ( ^21-^1 ~^11'^2^ 



^2“ (^11 ■*■^22^ (^21'^1 “^11-^2 ) ■*■■^2 (^11^22 “^21^12 ) 
Cj~- {a22p2~^^-ip2^ ^ 



Equation (2.11) can be also written as : 

i=ar+hh 



where : 




and 




In this way, the vehicle turning dynamics reduce to the 
approximate system of the following two equations : 



Tlf=r 



( 2 . 12 ) 



f =ar+b5 



(2,13) 



The control law then will be based on y and r and this is 
going to be in the form : 

6=Jc^ij;+ic2r (2,14) 



Solving for y from equations (2,12), (2.13) and (2.14) we end 

up with a second order differential equation as below : 
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ijr - ( a +2 )/c 2 ) ijr -jbiCj^i|; =0 



In terms of the desired natural frequency co^ and the damping 
ratio C the gains ki and k2 are given by the following 
relations : 




( 2 . 15 ) 



^ _ a+2Co>„ 

^2- ^ 



( 2 . 16 ) 



Therefore, the feedback gains for the control law can be 
determined based on the desired values of (On and 



C . GUIDANCE 

The heading autopilot that was designed through the 
control law in the previous section is called upon now to 
provide vehicle path keeping through a series of way points in 
the horizontal plane and as we will see later accurate path 
keeping along circular paths. In order to achieve it .and at 
the same time to keep the same heading autopilot design we 
have to use a suitable navigation scheme such as the line of 
sight guidance scheme (or the look ahead distance scheme) . In 
the above scheme the autopilot attempts to position the 
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longitudinal axis of the vehicle towards a point D which is 
located ahead of the vehicle's nominal path at a fixed 
distance d as shown in Figure 1. This fixed distance d is the 
look ahead distance and the line of sight angle O is defined 
as : 



tano = 



y 

d 



(2.17) 



For pure pursuit navigation the corresponding control law by 
modifying equation (2.14) becomes : 

+k2r (2.18) 



where : 



By introducing equation (2.17) in the control law equation, 
the commanded vehicle heading angle is not constant any more 
but a function of the vehicle's position y as it can be seen 
from equation (2.5). Since the control law was based on 
equations (2.3), (2.6) and (2.7) without including equation 
(2.5) for lateral displacement, the selected gains ki, kj do 
not guarantee stability any more and therefore conditions must 
be developed to guarantee stability and ensure satisfactory 
path keeping. 
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figure 1. Geometry and Definitions of Symbols 
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III. STABILITY 



A. APPROXIMATE CONDITION FOR STABILITY 

So far the complete system is given by the approximate 
equations (2.12), (2.13), and the inertial position rate 

equation (2.5). In its linearized and dimensionless form for 
a very small lateral velocity v this becomes : 

y=\|f (3.1) 



The control law including the guidance scheme is ; 

6 =ic^ ( + tan'l ) +Jc 2 X 



which in its linearized form is given by : 



(3.2) 



Solving equations (2.12), (2.13), (3.1) and (3.2) for y we end 
up to with a third order differential equation as follows : 

hk 

y- ia+hk^) y-bk^y — (3.3) 
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Writing the above equation in the s-domain and substituting kj 
and k 2 from equations (2.15) and (2.16), equation (3.3) takes 
the following form : 

s^+2Cw„s^+o)^s+-^=0 (3.4) 



The Routh criterion for stability, BC-AD > 0, where A, B, C 
and D are the coefficients of equation (3.4), applied to the 
above equation gives the following condition that must be 
satisfied : 




(3.5) 



If we have selected a natural frequency cOn and a damping ratio 
^ for the controller, it is possible from the above relation 
to have a quick estimate for the minimum look ahead distance 
d such that the system will be stable. In Figure 2 the 
approximate stability curve (dotted line) was plotted, the 
look ahead distance d versus the natural frequency of the 
controller <»„, based on equation (3.5) and for damping ratio 
^=0.8. This is in general considered to be good for a system, 
neither too sluggish, nor too stiff. As is discussed in the 
next section these values for the natural frequency and the 
damping ratio give also reasonable values for the gains k^ and 
k2. 
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d 



Figure 2 . Exact and Approximate Stability Curves for 
Various (Oq. 



12 




B. EXACT CONDITION FOR STABILITY 



For an exact calculation we use equations (2.3), (2.8), 

(2.9) and equation (2.5) which in linearized form appears as: 

y=\|f + v (3.6) 

This set completes the control law while the guidance scheme 
is given equation (3.2) . Substituting the control law into the 
equations (2.8), (2.9) and rearranging the terms the system 



becomes as shown : 

ijf=r (3.7) 

(3.8) 

i=k^h2'^ +a2-^v+ (a22+-^^2-^2) (3.9) 

y=\|f +v (3.10) 



Local stability properties are established by the eigenvalues 
of the stiffness matrix. The characteristic equation of this 
system is a fourth order polynomial in the form : 

AX^+SA^+CX2+DA.+^=0 (3.11) 



where : 
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c= + c *2 

P=I? 1 +I ?2 



with h, B, Cl, C2, Di, D2 and E defined as in the following 



A=1.0 



B=-(aii+a 22 ) -k^h^ 



^^11^22 ^12^21^ (^ll'^ 2 ~^ 21 '^l ^ -^2 ^ 1^2 



kiiJi 



*^ 2 = d 



I3i=(aiii?2-a2A>^i 



^ _ (a22i?i-ai2i>2)^i 
" d d“ 



(aiii?2-a2A)^i 

^ d 



Routh' s criterion for stability determines that loss of 
stability occurs when the following relation is satisfied : 

BCD-B^E-AD^=0 



After some algebra a quadratic equation for d occurs as below: 



aid^+a 2 d+a 3=0 



( 3 . 12 ) 
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where the coefficients aj, (X 2 , «3 are defined as follows : 

a2=sq£>2<i+BC2dDi-B2£:d-2 . OD^D^d 
aj=BC^dD^d-Dld^ 

The positive root of equation (3.12) determines the critical 
value of the look ahead distance d for stability. For every d 
> dcriticai the system is stable and the vehicle will follow the 
desired path. In the other case when d < dcriticai the system 
becomes unstable and the vehicle moves with an oscillatory 
motion as a result of a complex conjugate pair of eigenvalues 
with positive real parts that appears in this case. In Figure 
2 the continuous line represents the exact stability curve for 
various values of the (»„ versus the look ahead distance d and 
for a damping coefficient ^=0.8. As can be seen the 
approximate stability curve (dotted line) is very close to the 
exact and therefore it can be used as a good first 
approximation in order to provide adequate stability values to 
the system. In Figure 3 a family of curves was plotted for 
damping ratios with values of 1.2 to 0.4 with step 0.1 (left 
to right) for various values of 0^ versus the look ahead 
distance d. As can be seen the stability area is increasing 
for higher values of the natural frequency COn and for higher 
values of the damping ratio When the natural frequency 
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d 



Figure 3. Stability Curves for Various Damping Ratios 
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though reaches a value of 3.0 or higher, it is observed that 
the stability curves for the various damping ratios are 
approaching each other giving almost the same stability area. 
From Figure 4 it is also observed that for a natural frequency 
of 3.0 the gains k^, kj are about 3.2 and 1.2 respectively 
while higher values of (0^, give higher gains without much 
increase in the stability area. Therefore the above selection 
of cOn and ^ to be 3.0 and 0.8 respectively, is considered 
good. An other way to look at the gains is that for 1 degree 
of heading error a rudder of 3.2 degrees is required while for 
1 degree/sec turning rate a 1.2 rudder angle is required. 

C. SENSITIVITY OF STABILITY CURVES 

The hydrodynamic coefficients of the vehicle on which all 
the calculations were based have been determined 
experimentally. Therefore it is realistic to assume that a 
degree of error (uncertainty) that enters the calculations and 
may influence the results. In order to see how much this 
uncertainty for each coefficient changes the stability curves, 
a variation of 50% of the nominal value for each hydrodynamic 
coefficient was assumed and plotted versus the normalized 
look ahead distance d„ (dne„/d„o„) . The coefficients that found 
to affect stability were: Y«, Y^, N, and Nj. while the rest of 
the coefficients had no effect. In Figure 5 the variation of 
the coefficient Y, was plotted versus the normalized look 
ahead distance d„, and as it can be observed a variation of a 
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“n 



Figure 4 . 



Control Feedback Gains versus o^. 
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variation 




normalized d 



Figure 5. Changes of Critical Look Ahead Distance d for 
Variation of 



19 



50% of the nominal value gives a total change of about 0.40 
for d„. In Figure 6 the variation of versus d„ was plotted 
and for a variation 50% of a total change of about 0.37 of 
d„ occurred. In Figure 7 the variation for gives a total 
change of 0.14 for d„ while in Figure 8 the variation for Nj. 
gives a total change of 0.102 for d„. As can be seen the 
variation of Y^ and Y^ gives values for d„ of about four (4) 
times larger than and therefore these two coefficients 
affect the look ahead distance the most. For all the above 
plots above the values for natural frequency and damping ratio 
were 3.0 and 0.8 respectively. In an attempt to see how much 
a variation of ^ and ©n affects the previous results for Y<> and 
Yv the following Figures 9 , 10, 11 and 12 are plotted. It can 
be seen that the maximum deviation in the critical value of d 
for stability is 40% of its nominal value, while for most of 
the cases this deviation is well below 20%. 



20 



variation 




Figure 6. Changes of Critical Look Ahead Distance d for 
Variation of Y,. 
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variation 




normalized d 



Figure 7. Changes of Critical Look Ahead Distance d for 
Variation of N^. 
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variation 




Figure 8. Changes of Critical Look Ahead Distance d for 
Variation of N,. 
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variation 




0.0 0.9 1 1.1 1.2 1.3 1.4 



normalized d 



Figure 9. Sensitivity of Critical Look Ahead Distance for 
Variation in and for Different Dcunping Ratios. 
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variation 




0.7 0.75 0.8 0.85 0.9 0.95 1 1.05 1.1 1.15 



normilized d 



Figure 10. Sensitivity of Critical Look Ahead Distance for 
Variation in and for Different Damping 
Ratios . 
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variation 




0.8 0.9 1 1.1 1.2 1.3 1.4 1.5 



normalized d 



Figure 11. Sensitivity of Critical Look Ahead Distance for 
Variation in and for Different Natural 
Frequencies . 
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variation 




0.7 0.8 0.9 1 1.1 1.2 1.3 

normalized d 



Figure 12. Sensitivity of Critical Look Ahead Distance for 
Variation in and for Different Natural 
Frequencies . 
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IV. SHIP PATH KEEPING 



After satisfying the requirements for stability by 
selecting proper values for the gains k 2 and the look ahead 
distance d as was discussed in the previous section, the ship 
path keeping problem along straight line segments and circular 
trajectories is examined in this section. 

A. STRAIGHT LINE SEGMENTS PATH KEEPING 

So far the control law with the guidance scheme is given 
by equation (2.18) which in steady state gives a heading angle 
of zero (0) . In order for the vehicle to follow a straight 
line path between two given points (x^, y^) and (Xj, yj) the 

control law must be modified. If a, is the angle between the 
straight line (P) and the x axis; the distance y that enters 
the control law becomes y', as can be seen from Figure 13. The 
modified control law is shown below : 

fi=iCiSin (\|f-o-a) H-JcjX (4.1) 

where G and y ' are given by : 

o=^com=-arctan(^) 

y- (y~yi) cosa- (x-x^) sina 
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Figure 13. Guidance Scheme for Straight Line Path 
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At Steady state now the heading angle becomes a and the 
vehicle follows the straight line (P) . The simulation program 
takes into account all the possible different orientations of 
(P) by modifying the angle a properly for each case. In Figure 
14 we can see the simulated path of the vehicle (continuous 
line) and the reference path (dotted) for a two way point 
path. The reference line is a quarter of a circle while the 
two points have coordinates (0,0) and (10,10) respectively. In 
Figure 15 the rudder time history for the above two way point 
path keeping for a straight line is presented. The maximum 
value is about -23 degrees, which is a saturation point since 
the rudder is not allowed to exceed this value (exact 0.4 
radian or 22.923 degrees). In the following Figures 16 
through 23 the number of points was increased to 3, 5, 7 and 
13, in an effort to approach the reference path, by dividing 
the reference path in equally spaced arcs and following the 
straight line segments defined by these points. As can be 
seen, by increasing the number of points the vehicle can 
follow the reference path with increased accuracy. The 
drawbacks of simply increasing the number of discretization 
points are two. First, we are placing increased demands on 
storage and memory for the on-board processor. Second, even 
when satisfactory path accuracy is achieved by using a large 
number of way points the rudder activity is very excessive and 
erratic. Therefore if the vehicle must follow a circular arc 
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Figure 14 . Simulation for Reference and Vehicle Path 
for 2 Way Points . 
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Figure 15. Rudder Time History for 2 Hay Point Path. 



32 







0123456709 10 



z 



Figure 16. Simulation for Reference and Vehicle Path for 
3 Way Points. 
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0 2 4 6 0 10 12 14 16 
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Figure 17. Rudder Time History for 3 Way Point Path. 
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Fi^ro 18. Simulation for Reference and Vehicle Path for 
5 Way Points . 
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Figure 19. Rudder Time History for 5 Way Point Path. 
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Figure 20. Simulation for Reference and Vehicle Path for 
7 Way Points. 
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Figure 21. Rudder Time History for 7 Way Point Path. 
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Figure 22. Simulation for Reference and Vehicle Path for 
13 Way Points. 
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Figure 23. Rudder Time History for 13 Way Point Path. 
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path, doing so by following small straight line segments is 
not the best way since a large number of points is needed and 
the rudder is doing a considerable amount of effort. This 
means that a different way must be developed such that the 
vehicle at any time t is following the circular arc instead of 
a straight line. 



B. CIRCULAR ARC PATH KEEPING 

By demanding the vehicle to follow a certain circular path 
in a certain way, clockwise or counterclockwise, the angle G 
that enters the control law is going to vary depending on the 
orientation of the vehicle relative to the center of the path. 
It needs to change values from positive to negative in a 
consistent way with the control law requirements, in order for 
the vehicle to negotiate the assigned path. For this reason we 
need, to properly modify the control law for each case of the 
vehicle motion (cw, ccw) and for each quarter of the circle 
separately. Defining by A the distance from point V to point 
0, Figure 24, A is given by the following expression : 



A= [ (x-Xq) 2+ (y-yo) 2] ^ 



(4.2) 
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The radius of the circular path R can also be expressed as : 



J?= [ (Xi-Xq) 2+ (y^-y^) 2] 



2 



(4.3) 



From similar triangles the following relations yield the 
coordinate values of point 3 : 



Equations (4.2), (4.3), (4.4) and (4.5) are valid for CW or 

CCW motion and for any possible orientation of the vehicle. 
The equations that are valid for each separate case are 
presented below. 

1. CLOCKWISE MOTION 

a) For the fourth quarter of the circle and from Figure 24 
the angle 63 can be written as : 




(4.4) 



^ 3 =^ 0 +^ (y-vo) 



(4.5) 



03 =arcsin ( ^ 



(4.6) 



The angle 62 also appears as follows : 
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( 4 . 7 ) 



®2 = ’^-®l-®3 



where 0i can be written as : 






(4.8) 



For this case d is the length of the arc from point 3 to point 
4, while is the distance between points 3 and 4 and can be 
written as follows : 



d3,=2J?sin(-^) 



(4.9) 



The coordinates of point 4 now since dj^, Xj and yj are known 
can be evaluated from trigonometry as follows : 



(4.10) 

(4.11) 

The distance B from point V to point 4 is ; 



X4=X3+d3^sin02 
n=y3 +<^34^0802 
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_1 

S= [ (X-X 4 ) 2 + (y-y^) 2 



(4.12) 



Defining the angle y as following : 

Y=o+0i (4.13) 

and performing the law of sines in the triangle (0V4), y is 
given by : 

Y=arcsin[sin(-^) -^] (4.14) 



Substituting angle y, equation (4.13) gives angle a. For the 
control law the angle a (the angle that makes with the 
horizontal) is also needed and this is given by : 



a 




-02 



(4.15) 



Finally the control law takes the following form : 

6=kiSin (;|;-a+o) +^ 2 -^ (4.16) 

Taking the sine of the total heading angle accounts for the 
approximations of linearization that were made in selecting 
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the gains ki, k. 2 . The above form of the control law is valid 
for all of the quarters of the clockwise motion, while for the 
counterclockwise motion, angle o is reversing sign as we will 
see later. The equations (4.7), (4.8), (4.9), (4.12), (4.13) 
and (4.14) are also valid for all the cases. 

b) For the third quarter, the angle 63 is given by the 
following equation : 



03 =arcsin ( — ) 



The coordinates of point 4 are now given by the following 
expressions : 



x^=Xj-d^^cosQ2 

Finally, the angle a that enters the control law appears as 
below : 



a =71 -02 

c) For the second quarter, 63 is given by the following 
equation : 
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- . , X-, -Xf. , 

03=arcsin ( — ^ ° ) 



while x^, are given as : 



X4=X3-d34Sin02 



X4=y3 -^34^0802 



For this case, the angle a appear as : 



o = 




d) Similarly, for the case of the first quarter 



03=arcsin ( ) 

R 



x^=Xj+d^^cosd2 

y4=>^3-c?34sin02 

a = -02 
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2 . COUNTERCLOCKWISE MOTION 



From Figure 25 it can be seen that the equations 
(4.2), (4.3), (4.4), (4.5), (4.7), (4.8), (4.9), (4.12), 

(4.13), (4.14) continues to hold. 

a) For the fourth quarter. Figure 25, angle 63 , x^, and 
a appear as follows : 

03 =arcsin( 

^ R 

x^=Xj-d^^cosQ2 

y^4=>'3-^34sine2 



a=Tt +02 

The control law for this and also for the other three cases of 
the counterclockwise motion appears as below : 

6=ic3sin (ilf-0-0) +ic2r 

b) For the third quarter the following relations hold : 

03 =arcsin ( — ° — 

iv 

X4=X3+d34sin02 
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3^4 “ 3^3 



a- 




c) For the second quarter similarly we have : 



03=arcsin ( 

R 

X4=X3+d34COS02 

y4=y3+d34sin02 



0=0 



2 



d) Finally for the first quarter the following equations hold 



03=aicsin( ^ ^ ° 
x^ =X3 “Cf3^sin02 

y^=y3-c?3^cos02 

2 ^ 
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C. SIMULATION RESULTS 



By introducing the modifications of the previous chapter 
into the simulation program, the necessity of providing the 
simulation program with a set data, in order for the vehicle 
to perform the desired commands, occurred. The table below 
summarizes the required data, for the simulation program, for 
a test case of a figure - eight manoeuvre shown in Figure 28. 



X 


• 


TURN 


0 


PATH 


ICCW 


Xo 


Vo 


1#.0 


0.0 


0 


1.0 


0 


0 


0.0 


0.0 


15.0 


10.0 


0 


o.t 


0 


0 


0.0 


0.0 


10.0 


15.0 


0 


0.1 


1 


1 


1$.0 


10.0 


00 

O 


15.0 


0 


0.1 


0 


0 


0.0 


0.0 


3.0 


20.0 


0 


o.t 


1 


0 


e.o 


20.0 


CO 

o 


25.0 


0 


o.t 


1 


0 


e.o 


20.0 


13.0 


20.0 


0 


0.1 


1 


0 


8.0 


20.0 


8.0 


15.0 


0 


0.1 


t 


0 


0.0 


20.0 


c» 

o 


15.0 


0 


o.t 


0 


0 


0.0 


0.0 


1 . 0 


10.0 


0 


0.1 


1 


1 


6.0 


10 . 0 


6.0 


5.0 


0 


0.1 


1 


1 


6.0 


10.0 
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In the first two columns the coordinates x, y of the next way 
point are given. In the third column, the variable TURN 
selects how the vehicle is going to change its direction for 
the next way point in order to have a smooth change in course 
with less overshoot. There are two ways, 1 for circle, 0 for 
distance. When 1 is selected the vehicle must enter a circle 
with a preselected target radius centered at the way point 
before it starts heading for the next one. When 0 is selected 
the vehicle turns when it reaches a preselected target 
distance from the heading point. The fourth column, D, 
determines this preselected radius or distance that the 
vehicle has to approach the way point before it starts turning 
for the next one. The fifth column determines if the desired 
path is a circular arc (1) or a straight line segment (0) . In 
the sixth column, the (ICCW) index determines if the desired 
path is in the counterclockwise direction CCW for 1, or 
clockwise CW for 0. This is the case, of course, when the 
fifth column is 1 (circular path) . Finally, the two last 
columns give the coordinates of the center of the circular arc 
if this is the case. In an effort to make a comparison with 
the previous case of Figures 22 and 23, the vehicle path was 
plotted for a two way circular path. From Figure 26 can be 
seen that the reference path was achieved with the use of only 
two points instead of 13. From Figure 27 we can also see that 
the rudder steers the vehicle in a smooth way achieving much 
smaller values than any other case of straight line segments. 
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Figure 26. Actual and Reference Vehicle Path. 
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degrees 




0 2 4 6 0 10 12 14 16 

t 

Figure 27. Rudder Time History for the Simulation of 
Figure 26. 
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After introducing the previous table of data into the 
simulation program, a figure eight manoeuvre was performed by 
the vehicle, Figure 28. The rudder time history of the above 
manoeuvre was also plotted. Figure 29, The small values of the 
rudder angle even for a complicated manoeuvre like this prove 
the advantage of the method, over the use of line segments 
only . 
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X 



Figure 28. Figure - Eight (8) Manoeuvre. 



56 




0 10 20 30 40 50 60 70 80 90 



t 

Figure 29. Rudder Time History for the Manoeuvre of Figure 
28. 
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CONCLUSIONS AND RECOMMENDATIONS 



The main conclusions and contributions of this work can be 
summarized as follows : 

1. The use of Nomoto's model allows for a very efficient 
steering control design with excellent stability properties 
when coupled to a pursuit guidance scheme. 

2. An approximate stability condition was developed which 
is easy to compute and provides a very good estimate of the 
exact stability condition. 

3. Sensitivity analysis of the stability curves revealed 
the most critical hydrodynamic coefficients for stability. 

4 . The use of circular reference paths achieves smooth 
path response along curved segments with very reasonable 
rudder activity. 

Finally, some recommendations for future research include 
the analysis of path accuracy and disturbance rejection 
properties of the scheme, as well as the impact of sensor bias 
and noise. 
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APPENDIX A 



PROGRAM NOMO.FOR 



FOTIS A. PAPOULIAS - DIMITRIOS A. SIMAKIS 
NAVAL POSTGRADUATE SCHOOL 
MONTEREY CALIFORNIA 
JUNE 1992 

CRITICAL STABILITY PROGRAM 



REAL K1,K2,K3,L,NR,NV,NDRS,NDRB, I Z, MASS, 

& NRDOT,NVDOT 

C 

OPEN (10,FILE='NOMO.RES' , STATUS='NEW' ) 

C 

WEIGHT=435 . 0 

IZ =45.0 

L =7.3 

RHO =1.94 

G =32.2 

XG =0.0104 

MASS =WEIGHT/G 

MASS =MASS/ (0 . 5*RHO*L**3) 

IZ =IZ/ (0 .5*RHO*L**5) 

XG =XG/L 
C 

YRDOT =-0.00000 
YVDOT =-0.03430 
YR =+0.00000 
YV =-0.10700 
YDRS =+0.01241 
YDRB =+0.01241 
NRDOT =-0.00047 
NVDOT =-0.00000 
NR =-0.00390 
NV =-0.00000 
NDRS =-0.337*0.01241 
NDRB =+0.283*0.01241 
C 

DH =(IZ-NRDOT) * (MASS-YVDOT) - 
& ( MAS S*XG- YRDOT) * ( MAS S*XG -NVDOT) 

AA11= ( (IZ-NRDOT) *YV- ( MAS S*XG- YRDOT) *NV) /DH 
AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
& (MASS*XG-YRDOT) * ( -MASS*XG+NR) ) /DH 



59 



c 



c 



c 



c 



c 

c 



c 



c 



c 



AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 

BB11= ( (IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 

WRITE (*, 1006) 

READ (*,*) RATIO 

BB1=BB11+RATI0*BB12 

BB2=BB21+RATIO*BB22 



WRITE 
READ 
GO TO 
100 WRITE 
READ 
WRITE 
READ 
INCR= 
GO TO 
200 WRITE 
READ 
WRITE 
READ 
INCR= 



(*, 1001 ) 
(*, *) 
( 100 , 200 ) 
(*, 1002 ) 
(*, *) 

(*, 1003) 

(*,*) 

ZETA 

50 

(*, 1004) 
(*, *) 
(*,1005) 
(*, *) 



ICON 
, ICON 

ZMIN, ZMAX, I ZETA 
WN 

WNMIN, WNMAX, IWN 
ZETA 



50 Cl= (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 
C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) 

&+BB2* (AA11*AA22-AA21*AA12) 

C3=- (AA21*BB1-AA11*BB2) **2 

A=C1/C2 

B=C3/C2 



DO 1 I=1,INCR 

IF (ICON.EQ.l) ZETA=ZMIN + (ZMAX -ZMIN )* (I-l) / (INCR-1) 
IF (ICON.EQ.2) WN =WNMIN+ (WNMAX -WNMIN) * (I-l) / (INCR-1) 

IF (ICON.EQ.l) OUT=ZETA 
IF (ICON.EQ.2) OUT=WN 

K1=-WN*WN/B 
K2=-(A+2.0*ZETA*WN) /B 

B1P=- (AA11+AA22) -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 

& *K2-BB2*K1 

C2P=-BB1*K1 
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D1P= (AA11*BB2-AA21*BB1) *K1 
D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 
E2P= (AA11*BB2-AA21*BB1) *K1 
C 

AQ=B1P*C1P*D1P-D1P*D1P 

BQ=BlP*ClP*D2P+BlP*C2P*DlP-BlP*BlP*E2P-2 . 0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF (DET.LT.0.0) GO TO 1 

XD1= (-BQ+SQRT (DET) ) / (2 . 0*AQ) 

XD2= (-BQ-SQRT (DET) ) / (2 . 0*AQ) 

C 

C0EF1=BB1*AA22-BB2*AA12-BB2 

C0EF2=BB2*AA11-BB1*AA21 

C 

VAL1=1 . O+COEFl/ (XDl*COEF2) 

VAL2=1 . O+COEFl/ (XD2*COEF2) 

IF ( (VALI.LT.0.0) .OR. (XDI.LT.0.0) ) XD1=0.0 
IF ( (VAL2.lt. 0.0) .OR. (XD2.lt. 0.0) ) XD2=0.0 
XD3=1 .0/ (2.0*ZETA*WN) 

WRITE (10,2001) XD1,XD2,XD3,0UT,K1,K2 



1 


CONTINUE 










1001 


FORMAT 


(' 


ENTER 


1 : ZETA 


VARIATION' , /, 






& 


/ 




2 : WN 


VARIATION' ) 




1002 


FORMAT 


(' 


ENTER 


MIN, MAX, 


AND INCREMENTS OF 


ZETA' ) 


1003 


FORMAT 


(' 


ENTER 


WN' ) 






1004 


FORMAT 


(' 


ENTER 


MIN, MAX, 


AND INCREMENTS OF 


WN' ) 


1005 


FORMAT 


(' 


ENTER 


ZETA' ) 






1006 


FORMAT 


(' 


ENTER 


BOW/STERN RUDDER RATIO' ) 




2001 


FORMAT 


(6E15.5) 










END 
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APPENDIX B 



PROGRAM NOMOV.FOR 



FOTIS A. PAPOULIAS - DIMITRIOS A. SIMAKIS 
NAVAL POSTGRADUATE SCHOOL 
MONTEREY CALIFORNIA 
JUNE 1992 



SENSITIVITY OF HYDRODYNAMIC COEFFICIENTS PROGRAM 



REAL K1,K2,K3,L,NR,NV,NDRS,NDRB, I Z, MASS, 

& NRDOT,NVDOT 

C 

OPEN (10,FILE='NOMOV.RES' , STATUS='NEW' ) 

C 

WEIGHT=435.0 

IZ =45.0 

L =7.3 

RHO =1.94 

G =32.2 

XG =0.0104 

MASS =WEIGHT/G 

MASS =MASS/ (0 .5*RHO*L**3) 

IZ =IZ/ (0 . 5*RHO*L**5) 

XG =XG/L 
C 

YRDOT =-0.00000 
YVDOT =-0.03430 
YR =+0.00000 
YV =-0.10700 
YDRS =+0.01241 
YDRB =+0.01241 
NRDOT =-0.00047 
NVDOT =-0.00000 
NR =-0.00390 
NV =-0.00000 
NDRS =-0.337*0.01241 
NDRB =+0.283*0.01241 
C 

DH = (IZ-NRDOT) * (MASS-YVDOT) - 
& (MAS S*XG- YRDOT) * (MASS*XG-NVDOT) 

AA11= ( (IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
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& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 

AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 

BB11= ( (IZ-NRDOT) *YDRS~ (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 
C 

RATIO=-1.0 

C 

BB1=BB11+RATI0*BB12 

BB2=BB21+RATIO*BB22 

C 

ZETA=0 . 8 
WN=3.0 
C 

Cl= (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 

C2= (AA11+AA22) * (AA21*BB1-AA11*BB2) +BB2 
& * (AA11*AA22-AA21*AA12) 

C3=- (AA21*BB1-AA11*BB2) **2 
A=C1/C2 
B=C3/C2 
C 

K1=-WN*WN/B 
K2=- (A+2 . 0*ZETA*WN) /B 
C 

B1P=- (AA11+AA22) -BB2*K2 

C1P=(AA11*AA22-AA12*AA21) + (BB2*AA11-BB1*AA21) 

& *K2-BB2*K1 

C2P=-BB1*K1 

D1P= (AA11*BB2-AA21*BB1) *K1 
D2P= (BB1*AA22-BB2*AA12) *K1-BB2*K1 
E2P=(AA11*BB2-AA21*BB1) *K1 
C 

AQ=B1P*C1P*D1P-D1P*D1P 

BQ=BlP*ClP*D2P+BlP*C2P*DlP-BlP*BlP*E2P-2 . 0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF (DET.LT.0.0) GO TO 1 

XDNOM= (-BQ+SQRT (DET) ) / (2 . 0*AQ) 

C 

HOLD=YVDOT 

C 

DO 1 1=1,101 
C 

VARY=0 .5+ (I-l) /lOO.O 
YVDOT=0 . 5*HOLD+HOLD* (I-l) /lOO . 0 
C 

DH = (IZ-NRDOT) * (MASS-YVDOT) - 
& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 
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AA11= ( (IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 

AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 

BB11= ( (IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 
BB1=BB11+RATI0*BB12 
BB2=BB21+RATIO*BB22 



C 



B1P=- (AA11+AA22) -BB2*K2 

C1P= (AA11*AA22-AA12*AA21) + (BB2 *AAl 1-BBl *AA2 1 ) 
& *K2-BB2*K1 

C2P=-BB1*K1 

D1P= (AA11*BB2-AA21*BB1) *K1 

D2P= (BB1*AA22-BB2*AA12) *Kl-BB2*Kl 

E2P= (AA11*BB2-AA21*BB1) *K1 



AQ=B1P*C1P*D1P-D1P*D1P 

BQ=BlP*ClP*D2P+BlP*C2P*DlP-BlP*BlP*E2P-2 . 0*D1P*D2P 

CQ=B1P*C2P*D2P-D2P*D2P 

DET=BQ*BQ-4 . 0*AQ*CQ 

IF (DET.LT.0.0) GO TO 1 

XD1= (-BQ+SQRT (DET) ) / (2 . 0*AQ) 

XD2= (-BQ-SQRT (DET) ) / (2 . 0*AQ) 

C 

C0EF1=BB1 *AA22-BB2 *AA12-BB2 
COEF2=BB2 *AA1 1 -BBl *AA2 1 
C 

VAL1=1 . O+COEFl/ (XDl*COEF2) 

VAL2=1 . O+COEFl/ (XD2*COEF2) 

IF ( (VALl .LT. 0 . 0) .OR. (XDl .LT. 0 . 0) ) XD1=0.0 
IF ( (VAL2 .lt. 0.0) .OR. (XD2.lt. 0.0) ) XD2=0 . 0 
XD3=1 .0/(2 . 0*ZETA*WN) 

WRITE (10,2001) XDl/XDNOM, VARY 
1 CONTINUE 



C 

1001 FORMAT 
& 

1002 FORMAT 

1003 FORMAT 

1004 FORMAT 

1005 FOI3MAT 

1006 FORMAT 
2001 FORMAT 

END 



(' ENTER 

I 

(' ENTER 
( ' ENTER 
(' ENTER 
( ' ENTER 
(' ENTER 
(2E15.5) 



1 : ZETA VARIATION' , /, 

2 : WN VARIATION' ) 

MIN, MAX, AND INCREMENTS OF ZETA' ) 
WN' ) 

MIN, MAX, AND INCREMENTS OF WN' ) 
ZETA' ) 

BOW/STERN RUDDER RATIO') 
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APPENDIX C 



C PROGRAM SI MU, FOR 

C 

C 

C FOTIS A. PAPOULIAS - DIMITRIOS A. SIMAKIS 

C NAVAL POSTGRADUATE SCHOOL 

C MONTEREY CALIFORNIA 

C JUNE 1992 

C 

C SIMULATION PROGRAM FOR CONTROL AND GUIDANCE 

C ALONG STRAIGHT LINES AND 

C CIRCULAR TRAJECTORIES 

C 
C 

REAL K1,K2,L,NR,NV,NDRS,NDRB, IZ,MASS, 

& NRDOT,NVDOT 

C 

OPEN (10,FILE=' SIMU.RES' , STATUS='NEW' ) 

OPEN (11,FILE=' SIMU.DAT' , STATUS=' OLD' ) 

C 

C GEOMETRIC PROPERTIES 

C 

WEIGHT=435.0 

IZ =45.0 

L =7.3 

RHO =1.94 

G =32,2 

XG =0.0104 

MASS =WEIGHT/G 

MASS =MASS/ (0.5*RHO*L**3) 

IZ =IZ/ (0.5*RHO*L**5) 

XG =XG/L 
PI =3.1415926536 
C 

C HYDRODYNAMIC COEFFICIENTS 

C 

YRDOT =-0.00000 
YVDOT =-0.03430 
YR =+0.00000 
YV =-0.10700 
YDRS =+0.01241 
YDRB =+0.01241 
NRDOT =-0.00047 
NVDOT =-0.00000 
NR =-0.00390 
NV =-0.00000 
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CJ CJ CJ 



NDRS =-0.337*0.01241 
NDRB =+0.283*0.01241 
C 

DH = (IZ-NRDOT) * (MASS-YVDOT) - 
& (MASS*XG-YRDOT) * (MASS*XG-NVDOT) 

AA11= ( (IZ-NRDOT) *YV- (MASS*XG-YRDOT) *NV) /DH 
AA12= ( (IZ-NRDOT) * (-MASS+YR) - 
& (MASS*XG-YRDOT) * (-MASS*XG+NR) ) /DH 

AA21= ( (MASS-YVDOT) *NV- (MASS*XG-NVDOT) *YV) /DH 
AA22= ( (MASS-YVDOT) * (-MASS*XG+NR) - 
& (MASS*XG-NVDOT) * (-MASS+YR) ) /DH 

BB11= ( (IZ-NRDOT) *YDRS- (MASS*XG-YRDOT) *NDRS) /DH 
BB12= ( (IZ-NRDOT) *YDRB- (MASS*XG-YRDOT) *NDRB) /DH 
BB21= ( (MASS-YVDOT) *NDRS- (MASS*XG-NVDOT) *YDRS) /DH 
BB22= ( (MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 
C 

RATIO=-l 

C 

BBl=BBll+RATIO*BB12 

BB2=BB21+RATIO*BB22 

C 

ZETA=0 . 8 
WN=3.0 
DIST=1 .0 
C 

Cl= (AA11*AA22-AA21*AA12) * (AA21*BB1-AA11*BB2) 

C2= (AA11+AA22) * (AA21 *BB1-AA1 1 *BB2 ) +BB2 
& * (AA11*AA22-AA21*AA12) 

C3=- (AA21*BB1-AA11*BB2) **2 
A=C1/C2 
B=C3/C2 
C 

K1=-WN*WN/B 
K2=- (A+2 . 0*ZETA*WN) /B 
C 

STIME=150.0 
DELTAT=0.1 
ITIME=STIME/DELTAT 

INITIAL CONDITIONS 



PSI=0.0 
V=0 . 0 
R=0 . 0 
Y=0 . 0 
X=0 . 0 
X1=0 . 0 
Y1=0 . 0 
J=0 
C 

PRINT*, 'ENTER NUMBER OF POINTS' 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



READ*, NUM 

THE SIMULATION STARTS FROM THE POINT (0.0, 0.0) AND THE NEXT 
POINT (X2,Y2) IS PROVIDED BY THE DATA FILE. 



I TURN 

I TURN 1 
I TURN 0 
DD 

I PATH 

I PATH 1 

I PATH 0 

ICCW 0 
1 



DETERMINES THE WAY THE VEHICLE IS 
TO TURN TO PROCEED FOR THE NEXT POINT. 

FOR CIRCLE 

FOR DISTANCE 

DETERMINES THE DISTANCE OF THE VEHICLE 
FROM XZ,Y2 WHEN THE TURN STARTS. 

DETERMINES IF THE DESIRE PATH TO X2,Y2 
IS A CIRCULAR ONE OR A STRAIGHT LINE. 

FOR CIRCULAR 

FOR STRAIGHT LINE 

FOR CW 
FOR CCW 



X0,Y0 IS THE DESIRED CENTER OF THE CIRCULAR PATH 
AND X0,Y0 are provided FROM THE DATA FILE 
AND IF NOT THE DATA 
FILE PROVIDES ZERO VALUES. 



READ (11, *) X2, Y2, ITURN,DD, IPATH, ICCW,X0, YO 
SIMULATION STARTS 



C 



C 



93 



DO 1 I=1,ITIME 
TIME=I*DELTAT 



X21=X2-X1 

Y21=Y2-Y1 

IF ( (X21.EQ.0.) .AND. (Y21.GT.0 
IF ( (X21.EQ.0.) .AND. (Y21.lt. 0 
IF (X21.EQ.0.) GO TO 93 
ANA=ATAN (ABS (Y21) /ABS (X21) ) 

IF ( (X21.GE.0.) .AND. (Y21.GE.0 
IF ( (X21.GE.0.) .AND. (Y21.lt. 0 
IF ( (X21.lt. 0.) .AND. (Y21.GE.0 
IF ( (X21.lt. 0.) .AND. (Y21.lt. 0 
CONTINUE 



) ) 


ANA=+0 .5*PI 


) ) 


ANA=-0.5*PI 


) ) 


ANA= 


ANA 


) ) 


ANA= 


-ANA 


) ) 


ANA= PI -ANA 


) ) 


ANA=-PI+ANA 
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EQUATIONS OF MOTION 
PSIDOT=R 

VD0T=AA11*V+AA12*R+BB1*DR 
RD0T=AA2 1 *V+AA22 *R+BB2 *DR 
YDOT=SIN (PSD +V*COS (PSI) 

XDOT=COS (PSI) -V*SIN (PSI) 

FIRST ORDER INTEGRATION 

PSI=PSI+DELTAT*PSIDOT 

V=V+DELTAT*VDOT 

R=R+DELTAT*RDOT 

Y=Y+DELTAT*YDOT 

X=X+DELTAT*XDOT 

IF (PSI.GT. (2.0*PI) ) PSI=PSI-2.0*PI 

YPR= (Y-Yl) *COS (ANA) - (X-Xl) *SIN (ANA) 

PSIC=-ATAN (YPR/DIST) 

IF (IPATH.EQ.O) DR=K1*SIN ( (PSI-PSIC-ANA) ) +K2*R 



IF (IPATH.EQ.l) THEN 

DALF1= ( (X-XO) * (X-XO) + (Y-YO) * (Y-YO) ) **0.5 
RADIUS= ( (Xl-XO) * (Xl-XO) + (Yl-YO) * (Yl-YO) ) **0.5 
X3=X0+RADIUS/DALF1* (X-XO) 

Y3=Y0+RADIUS/DALF1* (Y-YO) 

THONE=PI/2 .-DIST/ (2 . *RADIUS) 

WRITE (*,*) X,Y,X3,Y3,X0,Y0, RADIUS 

IF (ICCW.EQ.O) THEN 
C 

IF ( (X21.GT.0.0) .AND. (Y21.GT.0.0) ) THEN 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2.0*RADIUS) ) 
X4=X3+DISTF*SIN (THTWO) 

Y4=Y3+DISTF*COS (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
IF (BALFA.GT.1.0) BALFA=1.0 
IF (BALFA.LE.1.0) AALFA=ASIN (BALFA) 

S I GMA=AALFA-THONE 
THTWO=0 . 5*PI-THTWO 
END IF 
C 

IF ( (X21 .GT. 0 . 0) .AND. (Y21 .LT. 0 . 0) ) THEN 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2.0*RADIUS) ) 
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X4=X3+DISTF*COS (THTWO) 

Y4=Y3-DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0 . 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
IF (BALFA.GT.l .0) BALFA=1.0 
IF (BALFA.LE. 1 . 0) AALFA=ASIN (BALFA) 

S I GMA=AALFA-THONE 
THTWO=-THTWO 
END IF 

IF ( (X21.lt. 0.0) .AND. (Y21.lt. 0.0) ) THEN 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3-DISTF*SIN (THTWO) 

Y4=Y3-DISTF*COS (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0 . 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
IF (BALFA.GT. 1 . 0) BALFA=1.0 
IF (BALFA.LE. 1.0) AALFA=AS IN (BALFA) 

S IGMA=AALFA-THONE 
THTWO=l . 5*PI-THTWO 
END IF 

IF ( (X21.lt. 0.0) .AND. (Y21.GT. 0.0) ) THEN 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=P I -THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3-DISTF*COS (THTWO) 

Y4=Y3+DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0 . 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
IF (BALFA.GT.l .0) BALFA=1.0 
IF (BALFA.LE. 1 .0) AALFA=AS IN (BALFA) 

S I GMA=AALFA- THONE 
THTWO=PI-THTWO 
END IF 



END IF 

IF (ICCW.EQ.l) THEN 

IF ( (X21 .GT.0.0) .AND. (Y21 .GT.0.0) ) THEN 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 

THTWO=P I -THONE-THTHR 

DISTF=2.0*RADIUS*SIN(DIST/ (2.0*RADIUS) ) 
X4=X3+DISTF*COS (THTWO) 

Y4=Y3+DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0 . 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 



5 

I 

5 

5 

5 
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IF (BALFA.GT. 1 . 0) BALFA=1 . 0 
IF (BALFA.LE.I .0) AALFA=ASIN (BALFA) 

S I GMA=AALFA-THONE 

THTWO=THTWO 

END IF 

IF ( (X21.GT. 0.0) .AND. (Y21.lt. 0.0) ) THEN 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2 . 0*RADIUS) ) 
X4=X3+DISTF*SIN (THTWO) 

Y4=Y3-DISTF*COS (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 
BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 
IF (BALFA.GT. 1 .0) BALFA=1.0 
IF (BALFA.LE.I .0) AALFA=AS IN (BALFA) 

S I GMA=AALFA-THONE 
THTWO=l . 5*PI+THTWO 
END IF 

IF ( (X21.lt. 0.0) .AND. (Y21.lt. 0.0) ) THEN 
THTHR=ASIN (ABS (Y0-Y3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2 . 0*RADIUS) ) 
X4=X3-DISTF*COS (THTWO) 

Y4=Y3-DISTF*SIN (THTWO) 

DALF2= ( (X-X4) * (X-X4) + (Y-Y4) * (Y-Y4) ) **0.5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT. 1.0) BALFA=1.0 

IF (BALFA. LE. 1.0) AALFA=AS IN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=PI+THTWO 

END IF 

IF ( (X21 .LT.0.0) .AND. (Y21.GT.0.0) ) THEN 
THTHR=ASIN (ABS (X0-X3) /RADIUS) 
THTWO=PI-THONE-THTHR 

DISTF=2 . 0*RADIUS*SIN (DIST/ (2 . 0*RADIUS) ) 
X4=X3-DISTF*SIN (THTWO) 

Y4=Y3+DISTF*COS (THTWO) 

DALF2=( (X-X4) * (X-X4 ) + ( Y-Y4 ) * (Y-Y4) ) **0.5 

BALFA=SIN (DIST/RADIUS) *DALF1/DALF2 

IF (BALFA.GT. 1 .0) BALFA=1.0 

IF (BALFA. LE. 1.0) AALFA=ASIN (BALFA) 

SIGMA=AALFA-THONE 

THTWO=0 . 5*PI+THTWO 

END IF 



C 



END IF 
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c 

c 



c 

c 
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IF (ICCW.EQ.O) DR=K1*SIN (PSI-THTWO+SIGMA) +K2*R 
IF (ICCW.EQ.l) DR=K1*SIN (PSI-THTWO-SIGMA) +K2*R 

END IF 



IF (DR.GT.0.4) DR=0.4 

IF (DR.LT.-0.4) DR=-0.4 

WRITE (10, 2001) TIME,X, Y, PSI,PSIC, DR, YPR 



TURN= (X-X2) * (X-X2) + (Y-Y2) * (Y-Y2) 

TURNC=SQRT (TURN) 

TURNL=SQRT (TURN-YPR* YPR) 

IF (ITURN.EQ.l) TD=TURNL 
IF (ITURN.EQ.O) TD=TURNC 
IF (TD.LT.DD) THEN 
J=J+1 

IF (J.EQ.NUM) GO TO 2002 
IF (J.NE.NUM) THEN 
X1=X2 
Y1=Y2 

READ (11, *) X2, Y2, ITURN,DD, IPATH, ICCW,X0, YO 
GO TO 1 
END IF 
END IF 



C 

1 CONTINUE 

2001 FORMAT (7E15.5) 

2002 STOP 
END 



PROGRAM REFERENCE PATH 

DIMITRIOS SIMAKIS 
NAVAL POSTGRADUATE SCHOOL 
MONTEFLEY, CALIFORNIA 
JUNE 1992 

OPEN (11,FILE=' SIMUR.REF' , STATUS=' OLD' ) 
OPEN (10, FILE=' SIMUR.RES' , STATUS='NEW' ) 
X1 = 0.0 
Y1 = 0 . 0 
PI=3 . 141 

PRINT*, 'ENTER NUMBER OF POINTS' 
READ*,NUM 
C 

DO 1 I=1,NUM+1 
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c 



c 



c 



c 



c 



c 



IF (I.EQ.l) THEN 

XR=X1 

YR=Y1 

WRITE (10,100) XR,YR 
GO TO 1 
END IF 

READ (11,*) X2, Y2, ITURN,DD, IPATH, ICCW,X0, YO 
THETA=0 . 0 

RADIUS= ( (Xl-XO) * (Xl-XO) + (Yl-YO) * (Yl-YO) ) **0 .5 

X21=X2-X1 

Y21=Y2-Y1 

DO 2 J=l,200 

THETA= THETA+0 . 01745 
IF ( IPATH. EQ.l) THEN 
IF (ICCW.EQ.O) THEN 

IF (X21.GT.0.0.AND.Y21.GT.0.0) THEN 
XR=XO-RADIUS*COS (THETA) 
YR=YO+RADIUS*SIN (THETA) 

END IF 

IF (X21.GT. 0.0. AND. Y21.lt. 0.0) THEN 
XR=XO+RADIUS*SIN (THETA) 
YR=YO+RADIUS*COS (THETA) 

END IF 

IF (X21.LT.0.0.AND.Y21.GT.0.0) THEN 
XR=XO-RADIUS*SIN (THETA) 
YR=YO-RADIUS*COS (THETA) 

END IF 

IF (X21.lt. 0.0. AND. Y21.lt. 0.0) THEN 
XR=XO+RADIUS*COS (THETA) 
YR=YO-RADIUS*SIN (THETA) 

END IF 
END IF 

IF (ICCW.EQ.l) THEN 

IF (X21.GT.0.0.AND.Y21.GT.0.0) THEN 
XR=XO+RADIUS*SIN (THETA) 
YR=YO-RADIUS*COS (THETA) 

END IF 

IF (X21.GT. 0.0. AND. Y21.lt. 0.0) THEN 
XR=XO-RADIUS*COS (THETA) 
YR=YO-RADIUS*SIN (THETA) 

END IF 

IF (X21.LT.O.O.AND.Y21.GT.O.O) THEN 
XR=XO+RADIUS*COS (THETA) 
YR=YO+RADIUS*SIN (THETA) 

END IF 
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c 



c 



2 

100 

1 



IF (X21 .LT. 0 . 0 .AND. Y21 .LT. 0 . 0) 
XR=XO-RADIUS*SIN (THETA) 
YR=YO+RADIUS*COS (THETA) 

END IF 
END IF 
END IF 

IF (IPATH.EQ.O) THEN 
XR=X2 
YR=Y2 
END IF 

WRITE (10,100) XR,YR 
DIS= ( (XR-X2) * (XR-X2) + (YR-Y2) * (YR- 
IF (DIS.LT.0.1) THEN 
X1=X2 
Y1=Y2 
GO TO 1 
END IF 
CONTINUE 
FORMAT (2E15.5) 

CONTINUE 

STOP 

END 



THEN 



Y2) ) **0.5 
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